#include "MT_Strickle_Alg.h"

#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

#include <assert.h>
#include <time.h>

using namespace cv;
using namespace std;

static MT_STRICKLE_ALG_PARAM strickle_params;
static bool IsInited = 0;


int OTSU(Mat srcImage) {
	int nCols = srcImage.cols;
	int nRows = srcImage.rows;
	int threshold = 0;
	//init the parameters
	int nSumPix[256];
	float nProDis[256];
	for (int i = 0; i < 256; i++)
	{
		nSumPix[i] = 0;
		nProDis[i] = 0;
	}

	//统计灰度集中每个像素在整幅图像中的个数
	for (int i = 0; i < nRows; i++)
	{
		for (int j = 0; j < nCols; j++)
		{
			nSumPix[(int)srcImage.at<uchar>(i, j)]++;
		}
	}

	//计算每个灰度级占图像中的概率分布
	for (int i = 0; i < 256; i++)
	{
		nProDis[i] = (float)nSumPix[i] / (nCols*nRows);
	}

	//遍历灰度级【0，255】，计算出最大类间方差下的阈值

	float w0, w1, u0_temp, u1_temp, u0, u1, delta_temp;
	double delta_max = 0.0;
	for (int i = 0; i < 256; i++)
	{
		//初始化相关参数
		w0 = w1 = u0 = u1 = u0_temp = u1_temp = delta_temp = 0;
		for (int j = 0; j < 256; j++)
		{
			//背景部分
			if (j <= i)
			{
				w0 += nProDis[j];
				u0_temp += j*nProDis[j];
			}
			//前景部分
			else
			{
				w1 += nProDis[j];
				u1_temp += j*nProDis[j];
			}
		}
		//计算两个分类的平均灰度
		u0 = u0_temp / w0;
		u1 = u1_temp / w1;
		//依次找到最大类间方差下的阈值
		delta_temp = (float)(w0*w1*pow((u0 - u1), 2)); //前景与背景之间的方差(类间方差)
		if (delta_temp > delta_max)
		{
			delta_max = delta_temp;
			threshold = i;
		}
	}
	return threshold;
}

bool lightdetection(vector<Point> pt, Mat image)
{
	if (pt.size() < 10) return false;
	Rect rt = boundingRect(pt);
	int x1 = rt.x, x2 = rt.x + (rt.width / 3), x3 = rt.x + (rt.width * 2 / 3), x4 = rt.x + rt.width, leftnum = 0, rightnum = 0;
	double grayleft = 0, grayright = 0;
	for (int i = 0; i < pt.size(); i++)
	{
		if (pt[i].x > x1&&pt[i].x < x2)
		{
			grayleft += image.ptr<uchar>(pt[i].y)[pt[i].x];
			leftnum++;
		}
		else if (pt[i].x > x3&&pt[i].x < x4)
		{
			grayright += image.ptr<uchar>(pt[i].y)[pt[i].x];
			rightnum++;
		}
	}
	grayleft /= leftnum;
	grayright /= rightnum;
	if (abs(grayleft - grayright) > 50)
		return true;
	else
		return false;
}

_EXPORT bool MT_Strickle_Alg_Init(MT_STRICKLE_ALG_PARAM initParam)
{
    strickle_params.sys_sens1=initParam.sys_sens1;
    strickle_params.sys_sens2=initParam.sys_sens2;
    strickle_params.inital_angle=initParam.inital_angle;
   	strickle_params.width_lth=initParam.width_lth;    //10
   	strickle_params.width_hth=initParam.width_hth;    //24.5
   	strickle_params.height_lth=initParam.height_lth;  //7.595
	strickle_params.rect = initParam.rect;
	strickle_params.downsample = initParam.downsample;
	IsInited = 1;
   	 return true;
}

_EXPORT bool MT_Strickle_Alg_SetParams(MT_STRICKLE_ALG_PARAM initParam)
{
	strickle_params.sys_sens1=initParam.sys_sens1;
	strickle_params.sys_sens2=initParam.sys_sens2;
	strickle_params.inital_angle=initParam.inital_angle;
   	strickle_params.width_lth=initParam.width_lth;    //10
   	strickle_params.width_hth=initParam.width_hth;    //24.5
   	strickle_params.height_lth=initParam.height_lth;  //7.595
	strickle_params.rect = initParam.rect;
	strickle_params.downsample = initParam.downsample;
	return true;
}

_EXPORT MT_STRICKLE_ALG_PARAM MT_Strickle_Alg_GetParams()
{
    MT_STRICKLE_ALG_PARAM re;
    re=strickle_params;
    return re;
}

_EXPORT void MT_Strickle_Alg_GetFrameInfo(MT_STRICKLE_INFO* info, unsigned char *pstFrameInfo, int w, int h)
{
	assert(pstFrameInfo != NULL);
	printf("S1\n");
	Mat image;
	image.create(h, w, CV_8UC1);
	memcpy(image.data,pstFrameInfo,w*h);

	cv::Rect ROIRect(cv::Rect(strickle_params.rect.x, 
							  strickle_params.rect.y, 
							  strickle_params.rect.w, 
							  strickle_params.rect.h));

	assert(ROIRect.x < w && ROIRect.x < h && (ROIRect.x + ROIRect.width) <= w && (ROIRect.y + ROIRect.height) <= h);
	cv::Mat roi = image(ROIRect).clone();
	cv::Mat ltroi = image(ROIRect).clone();

	int procwidth = roi.cols / strickle_params.downsample;
	int procheight = roi.rows / strickle_params.downsample;
	printf("S2,procwidth=%d procheight=%d\n", procwidth, procheight);
	cv::resize(roi, roi, cv::Size(procwidth, procheight));

	printf("otsu begin.\n");
	int otsut = OTSU(roi);
	//cv::threshold()
	printf("otsu end.\n");
	Mat bin2;
	threshold(roi, bin2, otsut, 255, CV_THRESH_BINARY);
	printf("S3 otsu thresh=>%d sys_sens1=>%d sys_sens2=>%d\n", otsut, strickle_params.sys_sens1, strickle_params.sys_sens2);
	otsut = 1;
	if (otsut < strickle_params.sys_sens2) {
		printf("S31\n");
        info->angle = 0;
		info->width = 0;
		info->height = 0;
		printf("return result.\n");
		return;
	}

	if(otsut > (strickle_params.sys_sens1+10)) {
		printf("S32\n");
		threshold(roi, bin2, strickle_params.sys_sens1+10, 255, CV_THRESH_BINARY);
	} else {
		printf("S33\n");
		threshold(roi, bin2, otsut, 255, CV_THRESH_BINARY);
	}
		

	vector<vector<Point>> contours; // Vector for storing contours

	printf("S4\n");
	findContours(bin2, contours, RETR_CCOMP, CHAIN_APPROX_SIMPLE); // Find the contours in the image
	double largest_area = 0;
	int largest_contour_index = 0;

	if (contours.size() == 0) {
		info->angle = -1.f;
		info->width = -1.f;
		info->height = -1.f;
		return;
	}

	for (size_t i = 0; i< contours.size(); i++) // iterate through each contour.
	{
		double area = contourArea(contours[i]);  //  Find the area of contour

		if (area > largest_area)
		{
			largest_area = area;
			largest_contour_index = i;               //Store the index of largest contour
			//bounding_rect = boundingRect(contours[i]); // Find the bounding rectangle for biggest contour
		}
	}
	int maxArea = largest_area;
	printf("S5 largest_contour_index=>%d contour_size=%d\n", largest_contour_index, contours.size());
	// 最大轮廓点数小于6？ 返回空？
	size_t count = contours[largest_contour_index].size();
	printf("count=>%d\n", count);
	if( count < 6 )
	{
        info->angle = -1.f;
		info->width = -1.f;
		info->height = -1.f;
		printf("S51\n");
		return;
	}
	printf("S6\n");
	RotatedRect box = fitEllipse(contours[largest_contour_index]);
	info->angle = (float)abs(90-box.angle);
	info->width = (float)box.size.width;
	info->height = (float)box.size.height;
	info->lightfault = lightdetection(contours[largest_contour_index],ltroi);
	return;
}

MT_STRICKLE_RESULT MT_Strickle_Alg_GetFrameResult(MT_STRICKLE_INFO FrameInfo)
{
    MT_STRICKLE_RESULT result;
	result.angle = FrameInfo.angle;
	double maxh = strickle_params.height_lth * strickle_params.rect.w / strickle_params.downsample;
	if(FrameInfo.height > maxh && FrameInfo.width > (strickle_params.width_lth + 8.5) && FrameInfo.width < (strickle_params.width_hth + 11.5))
	{
		if(result.angle < strickle_params.inital_angle)
		{
			result.status = 0;
		}
		else if(result.angle > 45)
			result.status = 2;
		else 
		{
			result.status = 3;
		}
		
		if(FrameInfo.lightfault)
			result.status += 16;
	}
	else if(FrameInfo.width > (strickle_params.width_hth + 11.5))
		result.status = 2;
	else
		result.status = 0;
	return result;
}

_EXPORT bool MT_Strickle_Alg_GetTestImg(unsigned char* src, unsigned char** dst, int &outputNum, int &width, int &height)
{
    if(IsInited)
    {
	Mat image;
	image.create(height, width, CV_8UC1);

	memcpy(image.data,src,height* width);

	cv::Rect ROIRect(cv::Rect(strickle_params.rect.x, strickle_params.rect.y, strickle_params.rect.w, strickle_params.rect.h));
	cv::Mat roi = image(ROIRect).clone();

	width = roi.cols / strickle_params.downsample;
	height = roi.rows / strickle_params.downsample;

	cv::resize(roi, roi, cv::Size(width, height));

	int otsut = OTSU(roi);
	Mat bin2;
	threshold(roi, bin2, otsut, 255, CV_THRESH_BINARY);

	if(otsut > strickle_params.sys_sens1+10)
		threshold(roi, bin2, strickle_params.sys_sens1+10, 255, CV_THRESH_BINARY);
	else if(otsut < strickle_params.sys_sens2)
		threshold(roi, bin2, strickle_params.sys_sens2, 255, CV_THRESH_BINARY);
//**************display************
	width =1280;
	height =720;
	Mat dis1( height, width, CV_8UC1);
	cv::resize(roi, dis1, cv::Size(width, height));

	char osdbuf[128];
	sprintf(osdbuf, "TL=%d  otsu=%d  TH=%d",strickle_params.sys_sens2, otsut, strickle_params.sys_sens1+10);
	cv::putText(dis1, osdbuf, cv::Point(0, 80), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 1);

	//Mat temp1,temp2;
	//cv::cvtColor(dis1, temp1, cv::COLOR_GRAY2BGR);
	//cv::cvtColor(temp1, temp2, cv::COLOR_BGR2YUV_I420);

	if(*dst!=NULL) 
	{ 
		delete *dst;
	}
	*dst = new unsigned char[width * height * 2];
	memcpy(*dst, dis1.data, (width * height));
//*********************************
	vector<vector<Point> > contours; // Vector for storing contours

	findContours(bin2, contours, RETR_CCOMP, CHAIN_APPROX_SIMPLE); // Find the contours in the image

	double largest_area = 0;
	int largest_contour_index = 0;
	for (size_t i = 0; i< contours.size(); i++) // iterate through each contour.
	{
		double area = contourArea(contours[i]);  //  Find the area of contour

		if (area > largest_area)
		{
			largest_area = area;
			largest_contour_index = i;               //Store the index of largest contour
			//bounding_rect = boundingRect(contours[i]); // Find the bounding rectangle for biggest contour
		}
	}

	if( contours.size() < 1 )
	{
        outputNum=1;
		return true;
	}

	int maxArea = largest_area;
	size_t count = contours[largest_contour_index].size();

	if( count < 6 )
	{
        outputNum=1;
		return true;
	}

	RotatedRect box = fitEllipse(contours[largest_contour_index]);
//**************display************
	ellipse(roi, box, Scalar(255, 255, 255), 1, CV_AA);

	Mat dis2(height, width, CV_8UC1);
	cv::resize(roi, dis2, cv::Size(width, height));

	sprintf(osdbuf, "Ra=%f  Rb=%f",box.size.height,box.size.width);
	cv::putText(dis2, osdbuf, cv::Point(0, 80), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 1);

	//cv::cvtColor(dis2, temp1, cv::COLOR_GRAY2BGR);
	//cv::cvtColor(temp1, temp2, cv::COLOR_BGR2YUV_I420);
	memcpy(*dst + (width * height), dis2.data, width * height);
//*********************************
	outputNum=2;
	return true;	
    }
    else
    	return false;
}

_EXPORT bool MT_Strickle_Alg_ReleaseTestImg(unsigned char** dst)
{
	if(*dst!=NULL) 
	{ 
		delete *dst;
		*dst=NULL;
	}
}

_EXPORT bool MT_Strickle_Alg_Release()
{

}
